/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * commtest.cxx
 *
 * PURPOSE:
 *  To test communications.
 *
 * LAST MODIFIED:
 * $Date: 2005/05/03 04:39:32 $
 * $Revision: 1.1.1.1 $
 * $Author: dixonc $
 ***********************************************************************/

#include <signal.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <netdb.h>

#include <iostream>

#include "socket.h"
#include "String.h"
#include "Thread.h"
#include "commTask.h"
#include "joystickTask.h"

// global variable area
ShmStruct ShmStructVar;

String joyCalib = "calibration.js0";
String joyAxes = "axes.js0";

//---------------------------------------------------------- 
// threads
//---------------------------------------------------------- 

void exitsignal( int signal )
{
	printf("\nCaught signal %d ... exiting\n",signal);
	fflush(stdout);
}

int main(int argc, char *argv[])
{
	JoystickTask joy("joystick");
	CommTask comm("comm");
	//---------------------------------------------------------- 
	// Catch Ctrl-c
	//---------------------------------------------------------- 
	signal( SIGINT, exitsignal);

	//---------------------------------------------------------- 
	// init the shared memory area
	//---------------------------------------------------------- 
	pthread_mutex_init(&ShmStructVar.mtx, NULL);

	//---------------------------------------------------------- 
	// init the different threads
	//---------------------------------------------------------- 

	String hostStr = "10.0.0.100";
	String portStr = "7070";
	String error = "";

	if( argc == 3) {
		hostStr = String(argv[1]);
		portStr = String(argv[2]);
	}

	error = "";
	cout << "Initializing threads ... " << endl;
	if ( comm.init(&ShmStructVar, hostStr, portStr, CommTask::Normal) == false )
		error = "Comm Init Error";
	else if ( joy.init(&ShmStructVar, joyCalib, joyAxes) == false)
		error = "Joystick Init Error";

	if( error != "" )
	{    
		cout << error << endl <<  "Exiting ..." << endl;
		// let default destructors deal with closing
		// system down properly since we havent started
		// threads
		exit(EXIT_FAILURE);
	}


	//---------------------------------------------------------- 
	// Start the different threads and runrates
	//---------------------------------------------------------- 
	cout << "Starting threads ... " << endl;
	if ( comm.start(1.0) != OK )
		cout << "Comm start Error" <<endl;
	if ( joy.start(25.0) != OK )
		cout << "Joy Start Error" <<endl;

	//---------------------------------------------------------- 
	// The main task can still hang out and do stuff
	//---------------------------------------------------------- 
	pause();

	//---------------------------------------------------------- 
	// Shutdown the threads
	//---------------------------------------------------------- 
	cout << "Stopping threads ... " << endl;
	joy.shutdown( );
	comm.shutdown( );

	// give them time
	sleep(1);

	//---------------------------------------------------------- 
	// Total program exit
	//---------------------------------------------------------- 
	cout << endl;
	exit(0); return 0;	 
}

